import math
from launch import LaunchDescription
from launch_ros.actions import Node

from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration

def generate_launch_description():
    """
    为每个机器人配置一个外部控制器和轨迹跟踪器
    创建局部路径规划器，生成局部轨迹
    创建核心管理器
    """

    state_controller = Node(
        package="controller",
        executable="tri_pid_lc",
        name="state_controller",
        output="screen",
        parameters=[
            {"gyro_pid": [0.1, 0.5, 0.0, 100.0]},
            {"angle_pid": [5.0, 0.0, 0.0, 100.0]},
            {"speed_pid": [0.0, 0.0, 0.0, 100.0]},
            {"rotate_pid": [0.2, 1.0, 0.0, 100.0]},
            {"state_rate": LaunchConfiguration("state_rate")}
        ],
    )

    follow_mpc_node = Node(
        package="controller_py",
        executable="mpc_path_tracker_lc",
        name="follow_controller",
        output="screen",
        parameters=[
            {"max_gyro": 30 / 180.0 * math.pi},
            {"mat_q": [10.0, 10.0, 10.0, 10.0]},
            {"mat_r": [1.0, 1.0]},
        ],
        remappings=[("odom", "sp_odom")],
    )

    local_path_planer = Node(
        package="path_planer",
        executable="local_path_planer_lc",
        name="local_planer",
        output="screen",
        parameters=[
            {"radius": 0.1},
            {"invert": False},
            {"start_vec": 0.5},
            {"dock_vec": 0.1},
        ],
        remappings=[
            ("odom_start", "sp_odom"),
            ("odom_end", "target_odom"),
            ("local_path", "reference_path"),
            ("local_vec", "reference_velocity"),
        ],
    )

    core = Node(
        package="core",
        executable="core",
        name="core",
        output="screen",
        parameters=[
            {
                "prefix": LaunchConfiguration("prefix"),
            }
        ],
    )

    return LaunchDescription(
        [
            DeclareLaunchArgument("prefix", default_value=""),
            DeclareLaunchArgument("state_rate", default_value="-1"),
            state_controller,
            follow_mpc_node,
            local_path_planer,
            core,
        ]
    )
